import pygame
import math

pygame.joystick.init()
joystick_count = pygame.joystick.get_count()
if joystick_count > 1:
    raise ValueError("Please Connect Just One Joystick")

joystick = pygame.joystick.Joystick(0)
joystick.init()

pygame.init()

def parse_wheels():
    numAxes = joystick.get_numaxes()
    jsInputs = [float(joystick.get_axis(i)) for i in range(numAxes)]

    # print (jsInputs)
    jsButtons = [float(joystick.get_button(i)) for i in range(joystick.get_numbuttons())]

            # Custom function to map range of inputs [1, -1] to outputs [0, 1] i.e 1 from inputs means nothing is pressed
            # For the steering, it seems fine as it is
    K1 = 1.0  # 0.55
    steerCmd = K1 * math.tan(1.1 * jsInputs[0])  #方向盘

    K2 = 1.6  # 1.6
    throttleCmd = K2 + (2.05 * math.log10(
        -0.7 * jsInputs[1] + 1.4) - 1.2) / 0.92
    if throttleCmd <= 0:   #油门
        throttleCmd = 0
    elif throttleCmd > 1:
        throttleCmd = 1

    brakeCmd = 1.6 + (2.05 * math.log10(
        -0.7 * jsInputs[2] + 1.4) - 1.2) / 0.92
    if brakeCmd <= 0:  #刹车
        brakeCmd = 0
    elif brakeCmd > 1:
        brakeCmd = 1
    print(steerCmd)

while True:
    for event in pygame.event.get():
        if event.type == pygame.JOYBUTTONDOWN:
            print(event.button)
    parse_wheels()
    pygame.time.wait(0)
